/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package org.jdesktop.wonderland.modules.kinectcontrol.client;

import java.io.BufferedReader;
import java.io.DataInputStream;
import java.io.FileInputStream;
import java.io.InputStream;
import java.io.InputStreamReader;
import java.util.ArrayList;
import java.util.logging.Level;
import java.util.logging.Logger;
import kinect.skeleton.Skeleton;
import org.jdesktop.wonderland.client.jme.JmeClientMain;
import org.jdesktop.wonderland.client.login.ServerSessionManager;
import org.jdesktop.wonderland.modules.contentrepo.client.ContentRepository;
import org.jdesktop.wonderland.modules.contentrepo.client.ContentRepositoryRegistry;
import org.jdesktop.wonderland.modules.contentrepo.common.ContentCollection;
import org.jdesktop.wonderland.modules.contentrepo.common.ContentResource;
import org.jdesktop.wonderland.modules.kinectcontrol.client.threads.KeyPressDispatcher;

/**
 *
 * @author Dominik
 */
public class SkeletonDataProcessor{

    /**
     * ArrayList of coordinates which are recorded in sequence to define one
     * gesture
     */
    private ArrayList<double[]> _video;
    
    /**
     * The minumum number of frames in the _video buffer before we attempt to start matching gestures
     */
    private static final int MINIMUMFRAMES = 6;
    
    /// <summary>
    /// How many skeleton frames to store in the _video buffer
    /// </summary>
    /**
     * How many skeleton frames to store in the _video buffer
     */
    private static final int BUFFERSIZE = 32;
    
    /**
     * Flag to show whether or not the gesture recogniser is capturing a new pose
     */
    private boolean _capturing;
    
    /**
     * Switch used to ignore certain skeleton frames
     */
    private int _flipFlop;
    
    /**
     * How many skeleton frames to ignore (_flipFlop)
     * 1 = capture every frame, 2 = capture every second frame etc.
     */
    private static final int Ignore = 2;
    
    /**
     * Dynamic Time Warping object
     */
    private DTWDetectionRecognizer _dtw;
    
    /**
     * Flags for reading DTW States
     */
    private boolean dtwRead = true;
    private boolean dtwCapture = true;
    private boolean dtwStore = true;
    
    private ContentRepository repo;
    private static final Logger LOGGER =
            Logger.getLogger(JmeClientMain.class.getName());
    
    /**
     * Dispatcher for Keypresses
     */
    KeyPressDispatcher pressDispatcher;
    
    
    /**
     *
     * @param source
     */
    public SkeletonDataProcessor(String gestureFile, boolean inRepo, ServerSessionManager serverSession) throws Exception {
        _dtw = new DTWDetectionRecognizer(12, 0.6, 2, 2, 10);
        _video = new ArrayList<double[]>();
        _capturing = false;
       
        repo = ContentRepositoryRegistry.getInstance().getRepository(serverSession);
        
        LoadGesturesFromFile(gestureFile, inRepo);
        
        pressDispatcher = new KeyPressDispatcher("modules/installed/kinect-control/client/keyMapping.txt", repo);
    }
    
    /**
     * Opens the sent text file and creates a _dtw recorded gesture sequence
     * 
     * @param fileLocation Full path to the gesture file
     */
    private void LoadGesturesFromFile(String fileLocation, boolean inRepo) throws Exception
    {
        int itemCount = 0;
        int lineCount = 0;
        String line;
        String gestureName = "";
        DataInputStream in = null;

        // TODO I'm defaulting this to 12 here for now as it meets my current need but I need to cater for variable lengths in the future
        ArrayList<double[]> frames = new ArrayList<double[]>();
        double[] items = new double[20];
        
        FileInputStream fstream = null;
        InputStream istream = null;
        BufferedReader br = null;
        
        //For reading from Repo
        ContentResource current = null;
        ContentCollection ccr = null;
        String strLine;
        
        
        // Read the file and display it line by line.
        if(inRepo){
            ccr = repo.getRoot();
            LOGGER.log(Level.INFO, "***" + ccr.getPath());
            //istream = this.getClass().getResourceAsStream(fileLocation);
            current = (ContentResource)ccr.getChild(fileLocation);
            br = new BufferedReader(new InputStreamReader(current.getInputStream()));
        }
        else{
            fstream = new FileInputStream(fileLocation);
            in = new DataInputStream(fstream);
            br = new BufferedReader(new InputStreamReader(in));
        }


        while ((line = br.readLine()) != null)
        {
            lineCount++;
            
            if (line.startsWith("@"))
            {
                gestureName = line;
                continue;
            }

            if (line.startsWith("~"))
            {
                frames.add(items);
                itemCount = 0;
                items = new double[20];
                continue;
            }

            if (!line.startsWith("----"))
            {
                boolean correct = true;
                double itemLine = 0;
                try{
                     itemLine = Double.parseDouble(line);
                }
                catch(Exception ex)
                {
                    correct = false;
                    LOGGER.logp(Level.SEVERE, this.getClass().getName().toString(), "LoadGesturesFromFile", "Error in gesture file. Line: " + lineCount);
                }
                
                if(correct)
                {
                    items[itemCount] = itemLine;
                    itemCount++;
                }
            }

            if (line.startsWith("----"))
            {
                _dtw.AddOrUpdate(frames, gestureName);
                frames = new ArrayList();
                gestureName = "";
                itemCount = 0;
            }
        }
        if(in != null)
            in.close();

        LOGGER.log(Level.FINE, "File loaded successfully: " + fileLocation);
    }    
    
    
    /**
     * Crunches Kinect SDK's Skeleton Data and spits out a format more useful
     * for DTW
     *
     * @param data Kinect SDK's Skeleton Data
     */
    public void processData(int id) {

        // Extract the coordinates of the points.
        Point3D[] p = new Point3D[10];
        //Point3D[] p = new Point3D[6];
        Point3D shoulderRight = new Point3D();
        Point3D shoulderLeft = new Point3D();
        
        try{
            p[0] = new Point3D(Skeleton.getSkeleton(id).getHandLeft().getPosition().x, Skeleton.getSkeleton(id).getHandLeft().getPosition().y, Skeleton.getSkeleton(id).getHandLeft().getPosition().z);
            p[1] = new Point3D(Skeleton.getSkeleton(id).getWristLeft().getPosition().x, Skeleton.getSkeleton(id).getWristLeft().getPosition().y, Skeleton.getSkeleton(id).getWristLeft().getPosition().z);
            p[2] = new Point3D(Skeleton.getSkeleton(id).getElbowLeft().getPosition().x, Skeleton.getSkeleton(id).getElbowLeft().getPosition().y, Skeleton.getSkeleton(id).getElbowLeft().getPosition().z);
            p[3] = new Point3D(Skeleton.getSkeleton(id).getKneeLeft().getPosition().x, Skeleton.getSkeleton(id).getKneeLeft().getPosition().y, Skeleton.getSkeleton(id).getKneeLeft().getPosition().z);
            p[4] = new Point3D(Skeleton.getSkeleton(id).getAnkleLeft().getPosition().x, Skeleton.getSkeleton(id).getFootLeft().getPosition().y, Skeleton.getSkeleton(id).getFootLeft().getPosition().z);
            p[5] = new Point3D(Skeleton.getSkeleton(id).getElbowRight().getPosition().x, Skeleton.getSkeleton(id).getElbowRight().getPosition().y, Skeleton.getSkeleton(id).getElbowRight().getPosition().z);
            p[6] = new Point3D(Skeleton.getSkeleton(id).getWristRight().getPosition().x, Skeleton.getSkeleton(id).getWristRight().getPosition().y, Skeleton.getSkeleton(id).getWristRight().getPosition().z);
            p[7] = new Point3D(Skeleton.getSkeleton(id).getHandRight().getPosition().x, Skeleton.getSkeleton(id).getHandRight().getPosition().y, Skeleton.getSkeleton(id).getHandRight().getPosition().z);
            p[8] = new Point3D(Skeleton.getSkeleton(id).getKneeRight().getPosition().x, Skeleton.getSkeleton(id).getKneeRight().getPosition().y, Skeleton.getSkeleton(id).getKneeRight().getPosition().z);
            p[9] = new Point3D(Skeleton.getSkeleton(id).getAnkleRight().getPosition().x, Skeleton.getSkeleton(id).getFootRight().getPosition().y, Skeleton.getSkeleton(id).getFootRight().getPosition().z);
            shoulderLeft = new Point3D(Skeleton.getSkeleton(id).getShoulderLeft().getPosition().x, Skeleton.getSkeleton(id).getShoulderLeft().getPosition().y, Skeleton.getSkeleton(id).getShoulderLeft().getPosition().z);
            shoulderRight = new Point3D(Skeleton.getSkeleton(id).getShoulderRight().getPosition().x, Skeleton.getSkeleton(id).getShoulderRight().getPosition().y, Skeleton.getSkeleton(id).getShoulderRight().getPosition().z);
        }
        catch(Exception ex)
        {
            LOGGER.logp(Level.SEVERE, this.getClass().getName().toString(), "processData", "Error reading joints " + ex.getMessage());
        }
        
        // Centre the data
        Point3D center = new Point3D((shoulderLeft.x + shoulderRight.x) / 2, (shoulderLeft.y + shoulderRight.y) / 2);

        for (int i = 0; i < p.length; i++) {    
            p[i].x -= center.x;
            p[i].y -= center.y;
        }

        // Normalization of the coordinates
        double shoulderDist =
                Math.sqrt(Math.pow((shoulderLeft.x - shoulderRight.x), 2)
                + Math.pow((shoulderLeft.y - shoulderRight.y), 2));
        for (int i = 0; i < p.length; i++) {
            p[i].x /= shoulderDist;
            p[i].y /= shoulderDist;
        }
       
        coorsReady2D(p);
    }

    /**
     * Runs every time our 2D coordinates are ready.
     *
     * @param a Skeleton 2D data Coord
     */
    private void coorsReady2D(Point3D[] a) {

        // We need a sensible number of frames before we start attempting to match gestures against remembered sequences
        if (_video.size() > MINIMUMFRAMES && _capturing == false) {
            ////Debug.WriteLine("Reading and video.Count=" + video.Count);
            String s = _dtw.Recognize(_video);
            if (!s.contains("__UNKNOWN")) {
                //TODO Add Event for senden the recognizes gesture
                LOGGER.log(Level.FINE, "**** Recognized as :" + s);
                pressDispatcher.gestureEventRegestered(s);
                // There was no match so reset the buffer
                _video = new ArrayList<double[]>();
            }
        }

        // Ensures that we remember only the last x frames
        if (_video.size() > BUFFERSIZE) {
            // Remove the first frame in the buffer
            _video.remove(0);
        }

        // Decide which skeleton frames to capture. Only do so if the frames actually returned a number. 
        // For some reason my Kinect/PC setup didn't always return a double in range (i.e. infinity) even when standing completely within the frame.
        // TODO Weird. Need to investigate this
        if (!Double.isNaN(a[0].x))
        {
            // Optionally register only 1 frame out of every n
            _flipFlop = (_flipFlop + 1) % Ignore;
            if (_flipFlop == 0)
            {
                _video.add(GetCoords(a));
            }
        }

        // Update the debug window with Sequences information
        //dtwTextOutput.Text = _dtw.RetrieveText();
    }
    
    /**
     * Gets the coordinates of our _points
     * 
     * @param points All Points
     * @return  The coordinates of our _points
     */
    private double[] GetCoords(Point3D[] points)
    {
        double[] tmp = new double[points.length * 2];
        for (int i = 0; i < points.length; i++)
        {
            tmp[2 * i] = points[i].x;
            tmp[(2 * i) + 1] = points[i].y;
        }

        return tmp;
    }
}
